#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

void test01() {
    Vector3f vect;
    vect << 1, 2, -3;
    cout << vect << endl;

    Vector2f vect2 = vect.head<2>();
    cout << vect2 << endl;
    cout << vect2.norm() << endl;
    cout << vect(2) << endl;
    cout << "=============" << endl;
    cout << abs(vect[2]) << endl;
}

void test02() {
    Vector3f vect;
    vect << 1, 1, 0;
    cout << vect << endl;

    Vector3f vect2;
    vect2 << 4, 5, 6;

    vect.head<2>() = vect2.head<2>();
    cout << vect << endl;

    Vector2f vect3;
    vect3 << 7, 8;
    vect.head<2>() = vect3;
    cout << vect << endl;
}

void test03() {
    cout << "===============" << endl;
    Vector3f vect;
    vect << 1, 1, -5;
    cout << vect << endl;

    Vector2f vect_hor = vect.head<2>();
    vect_hor.normalize();
    vect.head<2>() = vect_hor * 5;

    vect[2] = vect[2] / abs(vect[2]) * 3;

    cout << vect << endl;

    Eigen::Matrix3f shape_triangle{}; // 三角队形
    shape_triangle << 0.0, -1.0, -1.0, 0.0, -0.5, 0.5, 0.0, 0.0, 0.0;
    std::cout << shape_triangle << std::endl;
}

void test04() {
    Eigen::Matrix3f shape_triangle{}; // 三角队形
    for (int i = 0; i < 3; ++i) {
        shape_triangle.col(i) << 0.0, -1.0, -1.0;
    }
    std::cout << shape_triangle << std::endl;
}
int main(int argc, char *argv[]) {
    // test01();
    // test02();
    // test03();
    test04();
    return 0;
}
